#include "OpenGLHelpers.h"

bool glusInverseMatrixf( float matrix[16], const float source[16] )
{
	size_t i;

	int column;
	int row;

	float copy[16];

	glusLoadIdentityf(&matrix[0]);
	//
	// Copy the original matrix as we want to manipulate it
	//
	for (i = 0; i < 16; i++)
	{
		copy[i] = source[i];
	}

	//
	// Make triangle form
	//
	for (column = 0; column < 4; column++)
	{
		size_t row;

		for (row = column; row < 4; row++)
		{
			//
			// Is row all zero, then return false			
			//
			if (glusIsRowZerof(copy, row))
				return false;	

			//
			// Divide, if not zero, by copy[column*4+row]
			//
			if (copy[column*4+row] != 0.0f)
			{
				glusDevideRowByf(matrix, copy, row, copy[column*4+row]);
			}
		}

		//
		// Is column all zero, then return false
		//		
		if (glusIsColumnZerof(copy, column))
			return false;

		//
		// Is pivot [column*4+column] = 0.0f
		//
		for (row = column+1; row < 4; row++)
		{
			if (copy[column*4+row] == 1.0f)
			{
				//
				// Swap with pivot row = column
				//
				glusSwapRowf(matrix, copy, column, row);

				break;
			}
		}

		for (row = column+1; row < 4; row++)
		{
			//
			// Subtract, [column*4+row] not zero, current row minus pivot row = column
			//
			if (copy[column*4+row] != 0.0f)
			{
				glusAddRowf(matrix, copy, row, column, -1.0f);
			}
		}
	}

	//
	// Make diagonal form
	//
	for (column = 3; column >= 0; column--)
	{
		for (row = column-1; row >= 0; row--)
		{
			//
			// Subtract, if [column*4+row] not zero, current row minus pivot row = column with factor [column*4+row]
			//
			if (copy[column*4+row] != 0.0f)
			{
				glusAddRowf(matrix, copy, row, column, -copy[column*4+row]);
			}
		}
	}

	return true;
}
